#include "C_H.h"
#include "meun.h"

// 全局变量
uint8_t pagee = 0, menu_y1 = 1, menu_x1 = 0,menu_y1_last = 0,menu_x1_last = 0,menu_lie=0,menu_moshi=0,menu_lie_last=0;
//menu_moshi == 0  左右切换列数
//menu_moshi == 1  加减

uint8_t fy_flag = 0; // 0:不显示 1:显示
uint16_t fy_wan = 800;
uint16_t fy_zhi = 800;
uint8_t speed_choice = 0;


//测试函数
//adc


extern int cheku_1;

uint16_t adc_reg = 0;
float adc_scale = 0;
uint battery_vol = 0;


void duoji_test(void);
void adc_test(void);
void motor_test(void);
void gyro_test(void);
void save_parameter(void);
void load_parameter(void);
void speed_ring_test(void);
void image_test(void);
void fy_test(void);
extern int ima_change;
// 菜单初始化函数
void my_menu_init() {
    ips200_clear();
    if(menu_lie == 0) ips200_show_char( 0 , 10+menu_y1*20,'*');
    else if(menu_lie == 1) ips200_show_char( 115 , 10+menu_y1*20,'*');
    
    //std::cout << "menu_y1: " << static_cast<int>(menu_y1) << std::endl;
    if (pagee == 0) { // 主菜单
        #ifdef yidui
            ips200_show_string(30, 10, "----Main Pag--yidui----");
        #endif
        #ifdef erdui
            ips200_show_string(30, 10, "----Main Pag--erdui----");
        #endif
        //第一列                                            //第二列
        ips200_show_string(15, 30, "Turn_PID");            ips200_show_string(130, 30, "mohu_p");
        ips200_show_string(15, 50, "Speed_PID");           ips200_show_string(130, 50, "mohu_d");
        ips200_show_string(15, 70, "Run");                 ips200_show_string(130, 70, "tar_speed");
        ips200_show_string(15, 90, "Sai_Dao");                ips200_show_string(130, 90, "cuntu");
        ips200_show_string(15, 110, "switch");             ips200_show_string(130, 110, "show_ip");
        ips200_show_string(15, 130, "baohu");
        ips200_show_string(15, 150, "test");
        ips200_show_string(15, 170, "image");
        ips200_show_string(15, 190, "flash");
        ips200_show_string(15, 210, "Huan");
        
    } else if (pagee == 1) { // Tur_PID 子菜单

        ips200_show_string(15, 10, "  --Tur_PID--");
        ips200_show_string(15, 30, "C_Kp:"); ips200_show_float(70, 30, Curve_PID[0], 2, 3);  
        ips200_show_string(15, 50, "C_Kd:"); ips200_show_float(70, 50, Curve_PID[1], 2, 3);  
        ips200_show_string(15, 70, "dajiao:"); ips200_show_int(70, 70, dajiao, 2); 
        ips200_show_string(15, 90, "L_Kp:"); ips200_show_float(70, 90, Loc_PID[0], 2, 3);  
        ips200_show_string(15, 110, "L_Kd:"); ips200_show_float(70, 110, Loc_PID[1], 2, 3);  
        ips200_show_string(15, 130, "C_gKd:"); ips200_show_float(70, 130, Curve_PID[2], 2, 3);  
        ips200_show_string(15, 150, "L_gKd:"); ips200_show_float(70, 150, Loc_PID[2], 2, 3); 
        ips200_show_string(15, 170, "d_Kp:"); ips200_show_float(70, 170, Locd_PID[0], 2, 3);  
        ips200_show_string(15, 190, "d_Kd:"); ips200_show_float(70, 190, Locd_PID[1], 2, 3); 
    } else if (pagee == 2) { // Speed_PID 子菜单
        ips200_show_string(15, 10, "  --Speed_PID--");
        ips200_show_string(15, 30, "r_Kp:");ips200_show_float(70, 30, speed_r[0], 2, 3);  
        ips200_show_string(15, 50, "r_Ki:");ips200_show_float(70, 50, speed_r[1], 2, 3); 
        ips200_show_string(15, 70, "l_Kp:");ips200_show_float(70, 70, speed_l[0], 2, 3);  
        ips200_show_string(15, 90, "l_Ki:");ips200_show_float(70, 90, speed_l[1], 2, 3); 
        ips200_show_string(15, 110, "S_flag:");ips200_show_int(70, 110, speed_flag, 5); 
        ips200_show_string(15, 130, "ack_k:");ips200_show_float(70, 130, ackerman_k, 5 ,3); 
        ips200_show_string(15, 150, "si_l:");ips200_show_int(70, 150, speed_l_integralLimit, 5);   
        ips200_show_string(15, 170, "si_r:");ips200_show_int(70, 170, speed_r_integralLimit, 5);


        
    } else if (pagee == 3) { // duoji 子菜单
        ips200_show_string(15, 10, "  --Sai_Dao--");
        ips200_show_string(15, 30, "cheku_1:");ips200_show_int(80, 30, cheku_1, 5); 
        ips200_show_string(15, 50, "ima_change:");ips200_show_int(80, 50, ima_change, 5);
        ips200_show_string(15, 70, "Distance:");ips200_show_int(80, 70, Distance, 5); 
    } else if (pagee == 4) { // 开关 子菜单
        ips200_show_string(15, 10, "  --switch--");
        ips200_show_string(15, 30, "zhus_f:");ips200_show_uint(80, 30, zhushi_flag, 2); 
        ips200_show_string(15, 50, "sd_f:");ips200_show_uint(80, 50, sd_flag, 2); 
        ips200_show_string(15, 70, "serve_f:");ips200_show_uint(80, 70, serve_flag, 2); 
        ips200_show_string(15, 90, "banma_f:");ips200_show_uint(80, 90, banma_fache_flag, 2); 
        ips200_show_string(15, 110, "chasu_f:");ips200_show_uint(80, 110, chasu_flag, 2); 
        ips200_show_string(15, 130, "tiaos_f:");ips200_show_uint(80, 130, tiaoshi_flag, 2); 
        
    } else if (pagee == 5) { // 开关 子菜单
        ips200_show_string(15, 10, "  --baohu--");
        ips200_show_string(15, 30, "baohu_f:");ips200_show_uint(80, 30, switch_baohu, 2); 
        ips200_show_string(15, 50, "time:");ips200_show_float(80, 50, time_lock, 7 ,1);
        ips200_show_string(15, 70, "banma_c:");ips200_show_uint(80, 70, banma_jieshu_cnt, 2); 
        
    } else if (pagee == 6) { // 开关 子菜单
        ips200_show_string(15, 10, "  --test--");
        ips200_show_string(15, 30, "adc");//ips200_show_string(100, 30, "baohu");
        ips200_show_string(15, 50, "duoji");//ips200_show_uint(100, 50, time_lock, 3); 
        ips200_show_string(15, 70, "motor_encoder");
        ips200_show_string(15, 90, "gyro");
        ips200_show_string(15, 110, "speed_ring");
        ips200_show_string(15, 130, "image_test");
        ips200_show_string(15, 150, "fy_test");
    } else if (pagee == 7) { // 存图 子菜单
        ips200_show_string(15, 10, "  --image--");
        ips200_show_string(15, 30, "img_idx");ips200_show_int(80, 30, img_index, 5); 
        ips200_show_string(15, 50, "tcp_f");ips200_show_uint(80, 50, tcp_flag, 3); 
        ips200_show_string(15, 70, "tcp_s_f");ips200_show_uint(80, 70, tcp_suc_flag, 3); 
        ips200_show_string(15, 90, "min_th");ips200_show_uint(80, 90, otsuMinThreshold, 3); 
        ips200_show_string(15, 110, "max_th");ips200_show_uint(80, 110, otsuMaxThreshold, 3);   
        ips200_show_string(15, 130, "sd_fir");ips200_show_uint(80, 130, sd_first, 3);  
        ips200_show_string(15, 150, "read");   
        ips200_show_string(15, 170, "cuntu");  
        ips200_show_string(15, 190, "transfer");   
    } else if (pagee == 8) { // 存图 子菜单
        ips200_show_string(15, 10, "  --flash--");
        ips200_show_string(15, 30, "fangwuchu");
        ips200_show_string(15, 50, "save");//ips200_show_uint(100, 30, cunchu_cnt, 3); 
        ips200_show_string(15, 70, "load");
    } else if (pagee == 9) { //环pid
        ips200_show_string(15, 10, "  --huan_pid--");
        ips200_show_string(15, 30, "H1_Kp:"); ips200_show_float(70, 30, Huan1_PID[0], 2, 3); 
        ips200_show_string(15, 50, "H1_Kd:"); ips200_show_float(70, 50, Huan1_PID[1], 2, 3);
        ips200_show_string(15, 70, "H1_gKd:"); ips200_show_float(70, 70, Huan1_PID[2], 2, 3);
        ips200_show_string(15, 90, "H2_Kp:"); ips200_show_float(70, 90, Huan2_PID[0], 2, 3); 
        ips200_show_string(15, 110, "H2_Kd:"); ips200_show_float(70, 110, Huan2_PID[1], 2, 3);
        ips200_show_string(15, 130, "H2_gKd:"); ips200_show_float(70, 130, Huan2_PID[2], 2, 3);
        ips200_show_string(15, 150, "H3_Kp:"); ips200_show_float(70, 150, Huan3_PID[0], 2, 3); 
        ips200_show_string(15, 170, "H3_Kd:"); ips200_show_float(70, 170, Huan3_PID[1], 2, 3);
        ips200_show_string(15, 190, "H3_gKd:"); ips200_show_float(70, 190, Huan3_PID[2], 2, 3);
    } else if (pagee == 10) { 
        ips200_show_string(15, 10, "  --mohu_kp--");
        ips200_show_string(15, 30, "Kp_1:"); ips200_show_float(70, 30, Kp_UFF[0], 2, 3);  
        ips200_show_string(15, 50, "Kp_2:"); ips200_show_float(70, 50, Kp_UFF[1], 2, 3);
        ips200_show_string(15, 70, "Kp_3:"); ips200_show_float(70, 70, Kp_UFF[2], 2, 3);
        ips200_show_string(15, 90, "Kp_4:"); ips200_show_float(70, 90, Kp_UFF[3], 2, 3); 
        ips200_show_string(15, 110, "Kp_5:"); ips200_show_float(70, 110, Kp_UFF[4], 2, 3);
        ips200_show_string(15, 130, "Kp_6:"); ips200_show_float(70, 130, Kp_UFF[5], 2, 3);
        ips200_show_string(15, 150, "Kp_7:"); ips200_show_float(70, 150, Kp_UFF[6], 2, 3); 
    } else if (pagee == 11) { 
        ips200_show_string(15, 10, "  --mohu_kd--");
        ips200_show_string(15, 30, "Kd_1:"); ips200_show_float(70, 30, Kd_UFF[0], 2, 3); 
        ips200_show_string(15, 50, "Kd_2:"); ips200_show_float(70, 50, Kd_UFF[1], 2, 3);
        ips200_show_string(15, 70, "Kd_3:"); ips200_show_float(70, 70, Kd_UFF[2], 2, 3);
        ips200_show_string(15, 90, "Kd_4:"); ips200_show_float(70, 90, Kd_UFF[3], 2, 3); 
        ips200_show_string(15, 110, "Kd_5:"); ips200_show_float(70, 110, Kd_UFF[4], 2, 3);
        ips200_show_string(15, 130, "Kd_6:"); ips200_show_float(70, 130, Kd_UFF[5], 2, 3);
        ips200_show_string(15, 150, "Kd_7:"); ips200_show_float(70, 150, Kd_UFF[6], 2, 3); 
    } else if (pagee == 12) { 
        ips200_show_string(15, 10, "  --target_speed--");
        ips200_show_string(15, 30, "sp_cho:");ips200_show_uint(70, 30, speed_choice, 2); 
        ips200_show_string(15, 50, "hd_spd:");ips200_show_int(70, 50, huandao_speed, 5); 
        ips200_show_string(15, 70, "lz_spd:");ips200_show_int(70, 70, lz_speed, 5);
        ips200_show_string(15, 90, "dz_spd:");ips200_show_int(70, 90, dz_speed, 5);
        ips200_show_string(15, 110, "wan_spd:");ips200_show_int(70, 110, wan_speed, 5);
        //ips200_show_string(30, 130, "rp_speed:");ips200_show_int(100, 130, ramp_speed, 5);
    }
    
    
}

// 菜单命令处理函数
void my_menucmd(char key_num) {
    if (key_num == KEY_UP_M) { // 上
        menu_y1--;
        if (menu_y1 == 0) menu_y1 = 10;
        if (pagee == 3 && menu_y1 == 1) servo_base -= 2;


    } else if (key_num == KEY_DOWN_M) { // 下
        menu_y1++;
        if (menu_y1 > 10) menu_y1 = 1;
        if (pagee == 3 && menu_y1 == 1) servo_base += 2;


    } else if (key_num == KEY_LEFT_M) { // 左
        if (pagee == 1) { // Tur_PID
            if (menu_y1 == 1) Curve_PID[0] -= 0.05;
            else if (menu_y1 == 2) Curve_PID[1] -= 0.5;
            else if (menu_y1 == 3) dajiao -= 1;
            else if (menu_y1 == 4) Loc_PID[0] -= 0.05;   
            else if (menu_y1 == 5) Loc_PID[1] -= 0.5;     
            // else if (menu_y1 == 6) up_dj -= 1;
            // else if (menu_y1 == 7) down_dj -= 1;
            else if (menu_y1 == 6) Curve_PID[2] -= 0.01;
            else if (menu_y1 == 7) Loc_PID[2] -= 0.01;
            else if (menu_y1 == 8) Locd_PID[0] -= 0.05;
            else if (menu_y1 == 9) Locd_PID[1] -= 0.5;
            // else if (menu_y1 == 10) Huan_PID[0] -= 0.1;
            // else if (menu_y1 == 11) Huan_PID[1] -= 0.1;
            // else if (menu_y1 == 12) Huan_PID[2] -= 0.01;
            
        } else if (pagee == 2) { // Speed_PID
            if (menu_y1 == 1) speed_r[0] -= 1;
            else if (menu_y1 == 2) speed_r[1] -= 0.1;
            else if (menu_y1 == 3) speed_l[0] -= 1;
            else if (menu_y1 == 4) speed_l[1] -= 0.1;
            else if (menu_y1 == 5) speed_flag = 0;
            else if (menu_y1 == 6) ackerman_k -= 0.01;
            else if (menu_y1 == 7) speed_l_integralLimit -= 100;
            else if (menu_y1 == 8) speed_r_integralLimit -= 100;
        } else if (pagee == 3) {
            if (menu_y1 == 1) cheku_1-=100;
            else if (menu_y1 == 2) ima_change = 0;
            else if (menu_y1 == 3) Distance = 0;
        } else if (pagee == 4) {
            if (menu_y1 == 1) zhushi_flag = 0;
            else if (menu_y1 == 2) sd_flag = 0;
            else if (menu_y1 == 3) serve_flag = 0;
            else if (menu_y1 == 4) banma_fache_flag = 0; 
            else if (menu_y1 == 5) chasu_flag = 0;  
            else if (menu_y1 == 6) tiaoshi_flag -= 1;    
        } else if (pagee == 5) {
            if (menu_y1 == 1) switch_baohu = 0;
            else if (menu_y1 == 2) time_lock -= 500;
            else if (menu_y1 == 3) banma_jieshu_cnt -= 1;
            
        } else if (pagee == 7) {
            if (menu_y1 == 1) {img_index -= 100;if(img_index<0)img_index=0;}//cunchu_cnt -= 1;
            else if (menu_y1 == 2) tcp_flag = 0;
            else if (menu_y1 == 4) otsuMinThreshold -= 1;
            else if (menu_y1 == 5) otsuMaxThreshold -= 1;
            else if (menu_y1 == 6) sd_first = 0;
            
        } else if (pagee == 9) {
            if (menu_y1 == 1) Huan1_PID[0] -= 0.1;
            else if (menu_y1 == 2) Huan1_PID[1] -= 0.1;
            else if (menu_y1 == 3) Huan1_PID[2] -= 0.01;
            else if (menu_y1 == 4) Huan2_PID[0] -= 0.1;   
            else if (menu_y1 == 5) Huan2_PID[1] -= 0.1;    
            else if (menu_y1 == 6) Huan2_PID[2] -= 0.01;
            else if (menu_y1 == 7) Huan3_PID[0] -= 0.1;   
            else if (menu_y1 == 8) Huan3_PID[1] -= 0.1;    
            else if (menu_y1 == 9) Huan3_PID[2] -= 0.01;    
        } else if (pagee == 10) {
            if (menu_y1 == 1) Kp_UFF[0] -= 0.1;
            else if (menu_y1 == 2) Kp_UFF[1] -= 0.1;
            else if (menu_y1 == 3) Kp_UFF[2] -= 0.1;
            else if (menu_y1 == 4) Kp_UFF[3] -= 0.1;
            else if (menu_y1 == 5) Kp_UFF[4] -= 0.1;
            else if (menu_y1 == 6) Kp_UFF[5] -= 0.1;
            else if (menu_y1 == 7) Kp_UFF[6] -= 0.1;
        } else if  (pagee == 11) {
            if (menu_y1 == 1) Kd_UFF[0] -= 0.1;
            else if (menu_y1 == 2) Kd_UFF[1] -= 0.1;
            else if (menu_y1 == 3) Kd_UFF[2] -= 0.1;
            else if (menu_y1 == 4) Kd_UFF[3] -= 0.1;
            else if (menu_y1 == 5) Kd_UFF[4] -= 0.1;
            else if (menu_y1 == 6) Kd_UFF[5] -= 0.1;
            else if (menu_y1 == 7) Kd_UFF[6] -= 0.1;
        } else if  (pagee == 0) {
            menu_lie = 0; // 切换列数 
        } else if  (pagee == 12) {
            if (menu_y1 == 1) speed_choice -= 1;
            else if (menu_y1 == 2) huandao_speed -= 20;
            else if (menu_y1 == 3) lz_speed -= 20;
            else if (menu_y1 == 4) dz_speed -= 20;
            else if (menu_y1 == 5) wan_speed -= 20;
        }

        
        


    } else if (key_num == KEY_RIGHT_M) { // 右
        if (pagee == 1) { // Tur_PID
            if (menu_y1 == 1) Curve_PID[0] += 0.05;
            else if (menu_y1 == 2) Curve_PID[1] += 0.5;
            else if (menu_y1 == 3) dajiao += 1;
            else if (menu_y1 == 4) Loc_PID[0] += 0.05;   
            else if (menu_y1 == 5) Loc_PID[1] += 0.5;    
            // else if (menu_y1 == 6) up_dj += 1;
            // else if (menu_y1 == 7) down_dj += 1;
            else if (menu_y1 == 6) Curve_PID[2] += 0.01;
            else if (menu_y1 == 7) Loc_PID[2] += 0.01;
            else if (menu_y1 == 8) Locd_PID[0] += 0.05;
            else if (menu_y1 == 9) Locd_PID[1] += 0.5;            
            // else if (menu_y1 == 10) Huan_PID[0] += 0.1;
            // else if (menu_y1 == 11) Huan_PID[1] += 0.1;
            // else if (menu_y1 == 12) Huan_PID[2] += 0.01;
        } else if (pagee == 2) { // Speed_PID
            if (menu_y1 == 1) speed_r[0] += 1;
            else if (menu_y1 == 2) speed_r[1] += 0.1;
            else if (menu_y1 == 3) speed_l[0] += 1;
            else if (menu_y1 == 4) speed_l[1] += 0.1;
            else if (menu_y1 == 5) speed_flag = 1;
            else if (menu_y1 == 6) ackerman_k += 0.01;
            else if (menu_y1 == 7) speed_l_integralLimit += 100;
            else if (menu_y1 == 8) speed_r_integralLimit += 100;
        } else if (pagee == 3) {
            if (menu_y1 == 1) cheku_1+=100;
            else if (menu_y1 == 2) ima_change = 1;
            else if (menu_y1 == 3) fy_zhi += 25;
        } else if (pagee == 4) {
            if (menu_y1 == 1) zhushi_flag = 1;
            else if (menu_y1 == 2) sd_flag = 1;
            else if (menu_y1 == 3) serve_flag = 1;
            else if (menu_y1 == 4) banma_fache_flag = 1;     
            else if (menu_y1 == 5) chasu_flag += 1;  
            else if (menu_y1 == 6) tiaoshi_flag += 1;        
            
        } else if (pagee == 5) {
            if (menu_y1 == 1) switch_baohu = 1;
            else if (menu_y1 == 2) time_lock += 500;
            else if (menu_y1 == 3) banma_jieshu_cnt += 1;
        } else if (pagee == 7) {
            if (menu_y1 == 1) {img_index += 100;}//cunchu_cnt += 1;
            else if (menu_y1 == 2) tcp_flag = 1;
            else if (menu_y1 == 4) otsuMinThreshold += 1;
            else if (menu_y1 == 5) otsuMaxThreshold += 1;
            else if (menu_y1 == 6) sd_first = 1;
        } else if (pagee == 9) {
            if (menu_y1 == 1) Huan1_PID[0] += 0.1;
            else if (menu_y1 == 2) Huan1_PID[1] += 0.1;
            else if (menu_y1 == 3) Huan1_PID[2] += 0.01;
            else if (menu_y1 == 4) Huan2_PID[0] += 0.1;   
            else if (menu_y1 == 5) Huan2_PID[1] += 0.1;    
            else if (menu_y1 == 6) Huan2_PID[2] += 0.01;
            else if (menu_y1 == 7) Huan3_PID[0] += 0.1;   
            else if (menu_y1 == 8) Huan3_PID[1] += 0.1;    
            else if (menu_y1 == 9) Huan3_PID[2] += 0.01;  
        } else if (pagee == 10) {
            if (menu_y1 == 1) Kp_UFF[0] += 0.1;
            else if (menu_y1 == 2) Kp_UFF[1] += 0.1;
            else if (menu_y1 == 3) Kp_UFF[2] += 0.1;
            else if (menu_y1 == 4) Kp_UFF[3] += 0.1;
            else if (menu_y1 == 5) Kp_UFF[4] += 0.1;
            else if (menu_y1 == 6) Kp_UFF[5] += 0.1;
            else if (menu_y1 == 7) Kp_UFF[6] += 0.1;
        } else if  (pagee == 11) {
            if (menu_y1 == 1) Kd_UFF[0] += 0.1;
            else if (menu_y1 == 2) Kd_UFF[1] += 0.1;
            else if (menu_y1 == 3) Kd_UFF[2] += 0.1;
            else if (menu_y1 == 4) Kd_UFF[3] += 0.1;
            else if (menu_y1 == 5) Kd_UFF[4] += 0.1;
            else if (menu_y1 == 6) Kd_UFF[5] += 0.1;
            else if (menu_y1 == 7) Kd_UFF[6] += 0.1;
        } else if  (pagee == 0) {
            menu_lie = 1; // 切换列数 
        } else if  (pagee == 12) {
            if (menu_y1 == 1) speed_choice += 1;
            else if (menu_y1 == 2) huandao_speed += 20;
            else if (menu_y1 == 3) lz_speed += 20;
            else if (menu_y1 == 4) dz_speed += 20;
            else if (menu_y1 == 5) wan_speed += 20;
        }


    } else if (key_num == KEY_ENTER_M) { // 进入
        ips200_clear();
        if (pagee == 0) { // 主菜单
            menu_y1_last = menu_y1;
            menu_x1_last = menu_x1;
            menu_lie_last = menu_lie;
            if(menu_lie == 0)
            {
                menu_lie = 0; 
                if (menu_y1 == 1) pagee = 1; // 进入 Tur_PID
                else if (menu_y1 == 2) pagee = 2; // 进入 Speed_PID
                else if (menu_y1 == 3) 
                {
                    run_loop(); // 进入 Run
                    menu_y1 = menu_y1_last;
                    menu_x1 = menu_x1_last;
                    my_menu_init(); // 刷新菜单
                    my_menu_init();
                }
                else if (menu_y1 == 4) pagee = 3; //duoji
                else if (menu_y1 == 5) pagee = 4; //switch
                else if (menu_y1 == 6) pagee = 5; //baohu
                else if (menu_y1 == 7) pagee = 6; //test
                else if (menu_y1 == 8) pagee = 7; //image
                else if (menu_y1 == 9) pagee = 8; //flash
                else if (menu_y1 == 10) pagee = 9; //huan
                menu_y1 = 1;
            }
            else if(menu_lie == 1)
            {
                menu_lie = 0; 
                if (menu_y1 == 1) pagee = 10; 
                else if (menu_y1 == 2) pagee = 11; 
                else if (menu_y1 == 3) pagee = 12; 
                else if (menu_y1 == 4) system("echo '123' | sudo -S -E ./compress.sh"); //压缩图片
                else if (menu_y1 == 5) show_ip_on_display("wlan0", 30, 30); 
                
                menu_y1 = 1;
            }

        } 
        else if (pagee == 12){
        if (menu_y1 == 1)  set_speed_choice(speed_choice);
        } else if (pagee == 3){
            if (menu_y1 == 1)  pwm_set_duty(SERVO_PWM, servo_base);
        } else if (pagee == 6){
            if (menu_y1 == 1)  adc_test();
            if (menu_y1 == 2)  duoji_test();
            if (menu_y1 == 3)  motor_test();
            if (menu_y1 == 4)  gyro_test();
            if (menu_y1 == 5)  speed_ring_test();
            if (menu_y1 == 6)  image_test();
            if (menu_y1 == 7)  fy_test();
            
            
            
        } else if (pagee == 7){
            if (menu_y1 == 7)  bofang_image();
            if (menu_y1 == 8)  system("echo '123' | sudo -S -E ./compress.sh"); //压缩图片
            if (menu_y1 == 9)  system("echo '123' | sudo -S -E ./transfer.sh"); //传输图片
        } else if (pagee == 8){
            if (menu_y1 == 2)  save_parameter();
            if (menu_y1 == 3)  load_parameter();
            // if (menu_y1 == 1)  adc_test();
            // if (menu_y1 == 2)  duoji_test();
            // if (menu_y1 == 3)  motor_test();
            // if (menu_y1 == 4)  gyro_test();
        } 



    } else if (key_num == KEY_BACK_M) { // 退出
        ips200_clear();
        pagee = 0;
        menu_y1 = menu_y1_last;
        menu_x1 = menu_x1_last;
        menu_lie = menu_lie_last;
    }
    if(key_num!=0)
        my_menu_init(); // 刷新菜单
    
}

uint8_t detect_key() 
{
    if (!gpio_get_level(KEY_UP))
    {
        system_delay_ms(KEY_DelayTime);
        if (!gpio_get_level(KEY_UP))
        {
            return 1;
        }
    }

    if (!gpio_get_level(KEY_DOWN))
    {
        system_delay_ms(KEY_DelayTime);
        if (!gpio_get_level(KEY_DOWN))
        {
            return 2;
        }
    }

    if (!gpio_get_level(KEY_ENTER))
    {
        system_delay_ms(KEY_DelayTime);
        if (!gpio_get_level(KEY_ENTER))
        {
            return 3;
        }
    }

    if (!gpio_get_level(KEY_BACK))
    {
        system_delay_ms(KEY_DelayTime);
        if (!gpio_get_level(KEY_BACK))
        {
            return 4;
        }
    }

    if (!gpio_get_level(KEY_LEFT))
    {
        system_delay_ms(KEY_DelayTime);
        if (!gpio_get_level(KEY_LEFT))
        {
            return 5;
        }
    }

    if (!gpio_get_level(KEY_RIGHT))
    {
        system_delay_ms(KEY_DelayTime);
        if (!gpio_get_level(KEY_RIGHT))
        {
            return 6;
        }
    }
    return 0;
}


//测试函数
void adc_test(void)
{
    ips200_clear();
    while (!should_exit) 
    {
        char Key_Num;
        Key_Num = detect_key();
        // 检测返回键按下事件
        if (Key_Num == 4) {
            break;
        }
        adc_reg = adc_convert(ADC_REG_PATH);
        adc_scale = adc_get_scale(ADC_SCALE_PATH);
        battery_vol = adc_reg * adc_scale;//4.3
        ips200_show_string(30, 30, "vol:"); //ips200_show_uint(60, 30, battery_vol, 5); 
        ips200_show_float(100, 30, battery_vol*1.0, 7, 1); 
    }
}

//测试函数
void duoji_test(void)
{
    ips200_clear();
    uint16_t servo_zhi = servo_base;
    while (!should_exit) 
    {
        char Key_Num;
        Key_Num = detect_key();
        // 检测返回键按下事件
        if (Key_Num == 4) {
            pwm_set_duty(SERVO_PWM, 0);
            break;
        } else if (Key_Num == 1) {//jia
            servo_zhi += 2;       
        } else if (Key_Num == 2) {//jian
            servo_zhi -= 2;
        } else if (Key_Num == 5) {//jia
            servo_zhi += 30;       
        } else if (Key_Num == 6) {//jian
            servo_zhi -= 30;
        } else if (Key_Num == 3) {//queding
            servo_base = servo_zhi;
            ips200_show_string(30, 50, "set succed!");
            system_delay_ms(1000);
            ips200_show_string(30, 50, "             ");
        }
        pwm_set_duty(SERVO_PWM, servo_zhi);
        Ackerman_chasu(int(servo_zhi-servo_base));
        ips200_show_string(30, 30, "ser:"); ips200_show_uint(60, 30, servo_zhi, 4); 
        ips200_show_string(30, 50, "cha:"); ips200_show_int(60, 50, servo_zhi-servo_base, 4); 
    }
    pwm_set_duty(SERVO_PWM, 0);
}

//测试函数
void motor_test(void)
{
    ips200_clear();
    int16_t pwm_test=0;
    char index = 0;
    motor_test_flag = 1;
    while (!should_exit) 
    {
        char Key_Num;
        
        Key_Num = detect_key();
        // 检测返回键按下事件
        if (Key_Num == 4) {
            control_motors(0, 0);
            motor_test_flag = 0;
            break;
        } else if (Key_Num == 1) {//jia
            pwm_test += 1000;       
        } else if (Key_Num == 2) {//jian
            pwm_test -= 1000;
        } else if (Key_Num == 3) {//queding
            index = 1-index;
        }
        pwm_test = std::min(pwm_test, (int16_t)9000);
        pwm_test = std::max(pwm_test, (int16_t)-9000);
        if(index == 0)
        {
            control_motors(pwm_test, 0);
            ips200_show_string(30, 90, "left  "); 
        }
        else
        {
            control_motors(0, pwm_test);
            ips200_show_string(30, 90, "right"); 
        }
        
        ips200_show_string(30, 30, "pwm:"); ips200_show_int(120, 30, pwm_test, 4); 
        ips200_show_string(30, 50, "encoder_l:"); ips200_show_float(120, 50, current_speed_l, 4, 1);  
        ips200_show_string(30, 70, "encoder_r:"); ips200_show_float(120, 70, current_speed_r, 4, 1);  


        if (tcp_flag == 1)
        {
        seekfree_assistant_oscilloscope_data.data[0] = current_speed_l;//current_speed_l;
        seekfree_assistant_oscilloscope_data.data[1] = current_speed_r;//current_speed_r;
        seekfree_assistant_oscilloscope_data.data[2] = Left_encoder_data_kal;
        seekfree_assistant_oscilloscope_data.data[3] = Right_encoder_data_kal;
        seekfree_assistant_oscilloscope_data.data[4] = current_speed;
        seekfree_assistant_oscilloscope_data.data[5] = speed_setPoint;
        seekfree_assistant_oscilloscope_data.data[6] = speed_l_integral;
        seekfree_assistant_oscilloscope_data.data[7] = speed_r_integral;
        //        detector_oscilloscope_data.data[7] = 10000;

        // ���ñ�����Ҫ���ͼ���ͨ��������
        seekfree_assistant_oscilloscope_data.channel_num = 8;

        // �����������4��ͨ�������ݣ����֧��8ͨ��
        seekfree_assistant_oscilloscope_send(&seekfree_assistant_oscilloscope_data);
        }
        

    }
}

//测试函数
void speed_ring_test(void)
{
    ips200_clear();
    int speed_l_setPoint_test = 0;
    int speed_r_setPoint_test = 0;
    while (!should_exit) 
    {
        char Key_Num;
        Key_Num = detect_key();
        // 检测返回键按下事件
        if (Key_Num == 4) {
            control_motors(0, 0);
            break;
        } else if (Key_Num == 1) {//jia
            speed_l_setPoint_test += 100;       
        } else if (Key_Num == 2) {//jian
            speed_l_setPoint_test -= 100;
        } else if (Key_Num == 5) {//queding
            speed_r_setPoint_test += 100;       
        } else if (Key_Num == 6) {//queding
            speed_r_setPoint_test -= 100;    
        }
        pid_output_l = SpeedLPID_Calculate_wei(speed_l_setPoint_test, Left_encoder_data_kal);
        pid_output_r = SpeedRPID_Calculate_wei(speed_r_setPoint_test, Right_encoder_data_kal);
        control_motors(pid_output_l, pid_output_r);
        ips200_show_string(30, 30, "l_set:"); ips200_show_int(120, 30, speed_l_setPoint_test, 4); 
        ips200_show_string(30, 50, "r_set:"); ips200_show_int(120, 50, speed_r_setPoint_test, 4);
        // ips200_show_string(30, 70, "L_encod:"); ips200_show_int(120, 70, Left_encoder_data_kal, 4);
        // ips200_show_string(30, 90, "R_encod:"); ips200_show_int(120, 90, Right_encoder_data_kal, 4);
        // ips200_show_string(30, 110, "L_integ:");ips200_show_int(120, 110, speed_l_integral, 4);
        // ips200_show_string(30, 130, "R_integ:");ips200_show_int(120, 130, speed_r_integral, 4);
        // ips200_show_string(30, 150, "L_out:");ips200_show_int(120, 150, pid_output_l, 4);
        // ips200_show_string(30, 170, "R_out:");ips200_show_int(120, 170, pid_output_r, 4);

        if (tcp_flag == 1)
        {
        seekfree_assistant_oscilloscope_data.data[0] = current_speed_l;//current_speed_l;
        seekfree_assistant_oscilloscope_data.data[1] = current_speed_r;//current_speed_r;
        seekfree_assistant_oscilloscope_data.data[2] = Left_encoder_data_kal;
        seekfree_assistant_oscilloscope_data.data[3] = Right_encoder_data_kal;
        seekfree_assistant_oscilloscope_data.data[4] = current_speed;
        seekfree_assistant_oscilloscope_data.data[5] = speed_setPoint;
        seekfree_assistant_oscilloscope_data.data[6] = speed_l_integral;
        seekfree_assistant_oscilloscope_data.data[7] = speed_r_integral;
        //        detector_oscilloscope_data.data[7] = 10000;

        // ���ñ�����Ҫ���ͼ���ͨ��������
        seekfree_assistant_oscilloscope_data.channel_num = 8;

        // �����������4��ͨ�������ݣ����֧��8ͨ��
        seekfree_assistant_oscilloscope_send(&seekfree_assistant_oscilloscope_data);
        }

    }
}


void gyro_test(void)
{
    ips200_clear();
    while (!should_exit) 
    {
        char Key_Num;
        Key_Num = detect_key();
        // 检测返回键按下事件
        if (Key_Num == 4) {
            break;
        } else if (Key_Num == 1) {//jia

        } else if (Key_Num == 2) {//jian

        } else if (Key_Num == 3) {//queding

        }
        w_chasu();
        ips200_show_string(30, 30, "gyro_x:"); ips200_show_int(100, 30, imu_gyro_x, 5); 
        ips200_show_string(30, 50, "gyro_y:"); ips200_show_int(100, 50, imu_gyro_y, 5); 
        ips200_show_string(30, 70, "gyro_z:"); ips200_show_int(100, 70, imu_gyro_z, 5); 
        ips200_show_string(30, 90, "delt_v:"); ips200_show_int(100, 90, delt_v, 5); 
        
    }
}

void image_test(void)
{
    ips200_clear();
    dj_flag = 0;
    while (!should_exit) 
    {
        char Key_Num;
        Key_Num = detect_key();
        // 检测返回键按下事件
        if (Key_Num == 4) {
            break;
        } else if (Key_Num == 1) {//jia
            otsuMinThreshold += 1;
        } else if (Key_Num == 2) {//jian
            otsuMinThreshold -= 1;
        } else if (Key_Num == 5) {//jia
            otsuMaxThreshold += 1;       
        } else if (Key_Num == 6) {//jian
            otsuMaxThreshold -= 1;
        } else if (Key_Num == 3) {//queding
            //otsuMaxThreshold
        }
        ImageProcess();
        drawBoundaryPoints();
        getCompressedImage();
        
        
    }
}


// 计算无刷电调转速   （1ms - 2ms）/20ms * 10000（10000是PWM的满占空比时候的值）
// 无刷电调转速 0%   为 500
// 无刷电调转速 20%  为 600
// 无刷电调转速 40%  为 700
// 无刷电调转速 60%  为 800
// 无刷电调转速 80%  为 900
// 无刷电调转速 100% 为 1000
#ifdef yidui
void fy_test(void)
{
    ips200_clear();
    int16_t pwm_test=5000;
    //char index = 0;
    while (!should_exit) 
    {
        char Key_Num;
        
        Key_Num = detect_key();
        // 检测返回键按下事件
        if (Key_Num == 4) {
            pwm_set_duty(PWM_1, 5000);

            break;
        } else if (Key_Num == 1) {//jia
            pwm_test += 1000;       
        } else if (Key_Num == 2) {//jian
            pwm_test -= 1000;
        } 
        if(10000 < pwm_test)
        {
            pwm_test = 5000;
        }
        pwm_set_duty(PWM_1, pwm_test);
        ips200_show_string(30, 30, "pwm:"); ips200_show_int(120, 30, pwm_test, 4); 
        // ips200_show_string(30, 50, "encoder_l:"); ips200_show_float(120, 50, current_speed_l, 4, 1);  
        // ips200_show_string(30, 70, "encoder_r:"); ips200_show_float(120, 70, current_speed_r, 4, 1);  
        

    }
}
#endif

#ifdef erdui
void fy_test(void)
{
    ips200_clear();
    int16_t pwm_test=5000;
    //char index = 0;
    while (!should_exit) 
    {
        char Key_Num;
        
        Key_Num = detect_key();
        // 检测返回键按下事件
        if (Key_Num == 4) {
            pwm_set_duty(PWM_1, 5000);

            break;
        } else if (Key_Num == 1) {//jia
            pwm_test += 100;       
        } else if (Key_Num == 2) {//jian
            pwm_test -= 100;
        } 
        if(10000 <= pwm_test)
        {
            pwm_test = 9999;
        }
        pwm_set_duty(PWM_1, pwm_test);
        ips200_show_string(30, 30, "pwm:"); ips200_show_int(120, 30, pwm_test, 4); 
        // ips200_show_string(30, 50, "encoder_l:"); ips200_show_float(120, 50, current_speed_l, 4, 1);  
        // ips200_show_string(30, 70, "encoder_r:"); ips200_show_float(120, 70, current_speed_r, 4, 1);  
        

    }
}
#endif


void show_ip_on_display(const char *interface, int x, int y) {
    int fd;
    struct ifreq ifr;
    char ip_str[20];  // 存储IP地址字符串
    char display_str[50];  // 要显示的完整字符串
    
    fd = socket(AF_INET, SOCK_DGRAM, 0);
    if (fd < 0) {
        perror("socket");
        return;
    }

    // 指定要查询的网卡名称
    strncpy(ifr.ifr_name, interface, IFNAMSIZ-1);
    
    // 获取IPv4地址
    if (ioctl(fd, SIOCGIFADDR, &ifr) == -1) {
        perror("ioctl");
        close(fd);
        
        // 获取失败时显示错误信息
        snprintf(display_str, sizeof(display_str), "IP: N/A");
        ips200_show_string(x, y, display_str);
        return;
    }
    
    close(fd);
    
    // 格式化IP地址字符串
    const char *ip = inet_ntoa(((struct sockaddr_in *)&ifr.ifr_addr)->sin_addr);
    snprintf(display_str, sizeof(display_str), "IP: %s", ip);
    

    // 在IPS屏幕上显示
    ips200_show_string(x, y, display_str);
    
    // 同时在终端打印（调试用）
    printf("%s IP: %s\n", interface, ip);

    while (!should_exit) 
    {
        char Key_Num;
        Key_Num = detect_key();

        // 检测返回键按下事件
        if (Key_Num == 4) {
            break;
        }
    }
}